import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.RealMatrix;

import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3b class. The robot stops in front of the light bulb without
 * running into it. Proportional controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller3b extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	double[][] controllerMatrixData = { { 1d, 1d, 1d, 0d, 0d, 0d, 1d, 0d },
			{ 0d, 0d, 0d, 1d, 1d, 1d, 0d, 1d } };
	RealMatrix controllerMatrix = new Array2DRowRealMatrix(controllerMatrixData);

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		int sumSensorValues = 0;
		for (int i = 0; i < 8; i++) {
			sumSensorValues += getLightValue(i) / 9;
		}

		System.out.println("sumSensorValues: " + sumSensorValues);
		System.out.println("Sensor 0: " + getLightValue(0));
		System.out.println("Matrix 0: " + getLightValue(0) * sumSensorValues);

		double[][] sensorVectorData = { { getLightValue(0) / sumSensorValues },
				{ getLightValue(1) / sumSensorValues },
				{ getLightValue(2) / sumSensorValues },
				{ getLightValue(3) / sumSensorValues },
				{ getLightValue(4) / sumSensorValues },
				{ getLightValue(5) / sumSensorValues },
				{ getLightValue(6) / sumSensorValues },
				{ getLightValue(7) / sumSensorValues } };
		RealMatrix sensorVector = new Array2DRowRealMatrix(sensorVectorData);

		// Now multiply controllerMatrix by sensorMatrix
		RealMatrix motorVector = controllerMatrix.multiply(sensorVector);

		// Moterwerte reduzieren, damit er langsamer wird
		double[] subMatrixData = { 1.001d, 1.001d };
		RealMatrix subMatrix = new Array2DRowRealMatrix(subMatrixData);
		motorVector = motorVector.subtract(subMatrix);

		System.out.println(motorVector.getEntry(0, 0) + ","
				+ motorVector.getEntry(1, 0));

		setMotorSpeeds((int) motorVector.getEntry(0, 0),
				(int) motorVector.getEntry(1, 0));

	}
}